﻿/*                       NOTICE
 * 
 * This software was produced for the U. S. Government
 * under Contract No. FA8721-10-C-0001, and is
 * subject to the Rights in Noncommercial Computer Software
 * and Noncommercial Computer Software Documentation Clause
 * (DFARS) 252.227-7014 (JUN 1995)
 * 
 * Approved for Public Release: 11-2435
 * © 2011 The MITRE Corporation. All Rights Reserved. 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using Microsoft.Research.Kinect.Nui;
using TUIO;

namespace GRIP
{
    public class KinectAdapter : IAdapter
    {
        public enum FingerModes
        {
            Off,
            Synchronous,
            Asynchronous
        };

        [Setting]
        public FingerModes FingerMode;
        [Setting]
        public bool AllowInferredContacts;
        [Setting]
        public string Source;

        private AutoResetEvent[] ResetEvents = { new AutoResetEvent(false), new AutoResetEvent(false) };

        private Runtime Nui;
        private FingerFinder LeftFingerFinder;
        private FingerFinder RightFingerFinder;
        private byte[] DepthFrame = new byte[640 * 480 * 2];

        private long ContactID = 0;

        public TuioContact ThumbLeft;
        public TuioContact IndexFingerLeft;
        public TuioContact MiddleFingerLeft;
        public TuioContact RingFingerLeft;
        public TuioContact PinkyLeft;
        public TuioContact ThumbRight;
        public TuioContact IndexFingerRight;
        public TuioContact MiddleFingerRight;
        public TuioContact RingFingerRight;
        public TuioContact PinkyRight;
        public TuioContact HipCenter; 
        public TuioContact Spine;
        public TuioContact ShoulderCenter;
        public TuioContact Head;
        public TuioContact ShoulderLeft;
        public TuioContact ElbowLeft;
        public TuioContact WristLeft;
        public TuioContact HandLeft;
        public TuioContact ShoulderRight;
        public TuioContact ElbowRight;
        public TuioContact WristRight;
        public TuioContact HandRight;
        public TuioContact HipLeft;
        public TuioContact KneeLeft;
        public TuioContact AnkleLeft;
        public TuioContact FootLeft;
        public TuioContact HipRight;
        public TuioContact KneeRight;
        public TuioContact AnkleRight;
        public TuioContact FootRight;

        private List<TuioContact> contacts = new List<TuioContact>(30);

        private List<Action<TuioContact>> contactAddedListeners = new List<Action<TuioContact>>();
        private List<Action<TuioContact>> contactChangedListeners = new List<Action<TuioContact>>();
        private List<Action<TuioContact>> contactRemovedListeners = new List<Action<TuioContact>>();

        public List<TuioContact> getContacts()
        {
            return new List<TuioContact>(contacts);
        }

        public void registerContactAddedListener(Action<TuioContact> listener)
        {
            contactAddedListeners.Add(listener);
        }
        public void registerContactChangedListener(Action<TuioContact> listener)
        {
            contactChangedListeners.Add(listener);
        }
        public void registerContactRemovedListener(Action<TuioContact> listener)
        {
            contactRemovedListeners.Add(listener);
        }

        public KinectAdapter()
            : this(new KinectSettings(), new FingerSettings())
        {
        }

        public KinectAdapter(KinectSettings settings)
            : this(settings, new FingerSettings())
        {
        }

        public KinectAdapter(KinectSettings settings, FingerSettings fingerSettings)
        {
            Nui = new Runtime();
            FingerMode = settings.FingerMode;
            AllowInferredContacts = settings.AllowInferredContacts;
            Source = settings.Source;

            contacts.Add(ThumbLeft = new TuioContact(Source, ContactID++, 0, 0, 0, 0, TuioContactType.ThumbLeft));
            contacts.Add(IndexFingerLeft = new TuioContact(Source, ContactID++, 1, 0, 0, 0, TuioContactType.IndexFingerLeft));
            contacts.Add(MiddleFingerLeft = new TuioContact(Source, ContactID++, 2, 0, 0, 0, TuioContactType.MiddleFingerLeft));
            contacts.Add(RingFingerLeft = new TuioContact(Source, ContactID++, 3, 0, 0, 0, TuioContactType.RingFingerLeft));
            contacts.Add(PinkyLeft = new TuioContact(Source, ContactID++, 4, 0, 0, 0, TuioContactType.PinkyLeft));
            contacts.Add(ThumbRight = new TuioContact(Source, ContactID++, 5, 0, 0, 0, TuioContactType.ThumbRight));
            contacts.Add(IndexFingerRight = new TuioContact(Source, ContactID++, 6, 0, 0, 0, TuioContactType.IndexFingerRight));
            contacts.Add(MiddleFingerRight = new TuioContact(Source, ContactID++, 7, 0, 0, 0, TuioContactType.MiddleFingerRight));
            contacts.Add(RingFingerRight = new TuioContact(Source, ContactID++, 8, 0, 0, 0, TuioContactType.RingFingerRight));
            contacts.Add(PinkyRight = new TuioContact(Source, ContactID++, 9, 0, 0, 0, TuioContactType.PinkyRight));
            contacts.Add(HipCenter = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipCenter));
            contacts.Add(Spine = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.Spine));
            contacts.Add(ShoulderCenter = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderCenter));
            contacts.Add(Head = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.Head));
            contacts.Add(ShoulderLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderLeft));
            contacts.Add(ElbowLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ElbowLeft));
            contacts.Add(WristLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.WristLeft));
            contacts.Add(HandLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HandLeft));
            contacts.Add(ShoulderRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderRight));
            contacts.Add(ElbowRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ElbowRight));
            contacts.Add(WristRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.WristRight));
            contacts.Add(HandRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HandRight));
            contacts.Add(HipLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipLeft));
            contacts.Add(KneeLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.KneeLeft));
            contacts.Add(AnkleLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.AnkleLeft));
            contacts.Add(FootLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.FootLeft));
            contacts.Add(HipRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipRight));
            contacts.Add(KneeRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.KneeRight));
            contacts.Add(AnkleRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.AnkleRight));
            contacts.Add(FootRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.FootRight));

            try
            {
                Nui.Initialize(RuntimeOptions.UseDepth | RuntimeOptions.UseSkeletalTracking);
                Nui.SkeletonEngine.TransformSmooth = true;
                Nui.SkeletonEngine.SmoothParameters = new TransformSmoothParameters
                {
                    Smoothing = 1.0f,
                    Correction = 0.0f,
                    Prediction = 0.5f,
                    JitterRadius = 0.0f,
                    MaxDeviationRadius = 0.0f
                };
            }
            catch (InvalidOperationException)
            {
                throw new InvalidOperationException("Runtime initialization failed. Please make sure Kinect device is plugged in.");
            }

            try
            {
                Nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution640x480, ImageType.Depth);
            }
            catch (InvalidOperationException)
            {
                throw new InvalidOperationException("Failed to open stream. Please make sure to specify a supported image type and resolution.");
            }

            LeftFingerFinder = new FingerFinder(ResetEvents[0], this, FingerFinder.HandType.Left, fingerSettings);
            RightFingerFinder = new FingerFinder(ResetEvents[1], this, FingerFinder.HandType.Right, fingerSettings);

            Nui.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);
            Nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);
        }
            
        private void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            e.ImageFrame.Image.Bits.CopyTo(DepthFrame, 0);
        }

        private void getJointPosition(Joint joint, TuioContact contact)
        {
            if (joint.TrackingState == JointTrackingState.Tracked || (AllowInferredContacts && joint.TrackingState == JointTrackingState.Inferred))
            {
                float depthX, depthY;
                int scaledDepthX, scaledDepthY;
                Nui.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
                depthX = Math.Max(0, Math.Min(depthX, 1));
                depthY = Math.Max(0, Math.Min(depthY, 1));
                scaledDepthX = 639 - (int)(depthX * 639);
                scaledDepthY = (int)(depthY * 479);
                int index = (scaledDepthY * 640 + scaledDepthX) * 2;

                int realDepth = (DepthFrame[index + 1] << 8) | DepthFrame[index];
                if(realDepth == 0)
                {
                    if (contact.active)
                    {
                        contact.active = false;
                        runContactRemovedListeners(contact);
                    }
                    return;
                }

                contact.xpos = depthX;
                contact.ypos = depthY;
                contact.zpos = realDepth;
                if (!contact.active)
                {
                    contact.active = true;
                    contact.session_id = ContactID++;
                    runContactAddedListeners(contact);
                }
                else
                    runContactChangedListeners(contact);
            }
            else if(contact.active)
            {
                contact.active = false;
                runContactRemovedListeners(contact);
            }
        }

        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            JointsCollection joints;
            SkeletonFrame skeletonFrame = e.SkeletonFrame;

            foreach(SkeletonData data in skeletonFrame.Skeletons)
                if (SkeletonTrackingState.Tracked == data.TrackingState)
                {
                    joints = data.Joints;
                    getJointPosition(joints[JointID.HandLeft], HandLeft);
                    if (FingerMode != FingerModes.Off && HandLeft.active)
                        ThreadPool.QueueUserWorkItem(LeftFingerFinder.processHand, ResetEvents[0]);
                    getJointPosition(joints[JointID.HandRight], HandRight);
                    if (FingerMode != FingerModes.Off && HandRight.active)
                        ThreadPool.QueueUserWorkItem(RightFingerFinder.processHand, ResetEvents[1]);
                    if (FingerMode == FingerModes.Synchronous)
                    {
                        ResetEvents[0].WaitOne(100);
                        ResetEvents[1].WaitOne(100);
                    }
                    getJointPosition(joints[JointID.HipCenter], HipCenter);
                    getJointPosition(joints[JointID.Spine], Spine);
                    getJointPosition(joints[JointID.ShoulderCenter], ShoulderCenter);
                    getJointPosition(joints[JointID.Head], Head);
                    getJointPosition(joints[JointID.ShoulderLeft], ShoulderLeft);
                    getJointPosition(joints[JointID.ElbowLeft], ElbowLeft);
                    getJointPosition(joints[JointID.WristLeft], WristLeft);
                    getJointPosition(joints[JointID.ShoulderRight], ShoulderRight);
                    getJointPosition(joints[JointID.ElbowRight], ElbowRight);
                    getJointPosition(joints[JointID.WristRight], WristRight);
                    getJointPosition(joints[JointID.HipLeft], HipLeft);
                    getJointPosition(joints[JointID.KneeLeft], KneeLeft);
                    getJointPosition(joints[JointID.AnkleLeft], AnkleLeft);
                    getJointPosition(joints[JointID.FootLeft], FootLeft);
                    getJointPosition(joints[JointID.HipRight], HipRight);
                    getJointPosition(joints[JointID.KneeRight], KneeRight);
                    getJointPosition(joints[JointID.AnkleRight], AnkleRight);
                    getJointPosition(joints[JointID.FootRight], FootRight);
                }
        }

        public Runtime getRuntime()
        {
            return Nui;
        }

        public byte[] getDepthFrame()
        {
            return DepthFrame;
        }

        public void runContactAddedListeners(TuioContact contact)
        {
            foreach (Action<TuioContact> listener in contactAddedListeners)
                listener(contact);
        }

        public void runContactChangedListeners(TuioContact contact)
        {
            foreach (Action<TuioContact> listener in contactChangedListeners)
                listener(contact);
        }

        public void runContactRemovedListeners(TuioContact contact)
        {
            foreach (Action<TuioContact> listener in contactRemovedListeners)
                listener(contact);
        }

        public void shutdown()
        {
            Nui.Uninitialize();
        }
    }
}
